#pragma once
#include <xmmintrin.h>
#include <intrin.h>
#include <math.h>

#pragma intrinsic (_mm_hadd_ps)

namespace sse_math
{
#define EPSILON 0.00000000001f

//	static const __m128 g_sgn_mask_vct = _mm_set_ps(0.0f, 0.0f, 0.0f, 1.0f);
	class CVector3
	{
	private:
		__m128 _v;
	public:
		CVector3(const float x, const float y, const float z){_v = _mm_setr_ps(x, y, z, 0.0f);}
		CVector3(const __m128& v):_v(v){}
		CVector3(const CVector3& v):_v(v._v){}
		CVector3(){_v = _mm_setr_ps(0.0f, 0.0f, 0.0f, 0.0f);}

		__inline float magnitude(){return sqrt(dot(*this));}
		__inline CVector3& operator = (const CVector3& v) {_v = v._v; return *this;}

		/* Addition */
		__inline CVector3& operator += (const CVector3& v) {_v = _mm_add_ps(_v, v._v); return *this;}
		__inline CVector3& operator += (const __m128 & rr) {_v = _mm_add_ps(_v, rr); return *this;}
		__inline CVector3& operator += (const float f)
		{
			__m128 t = _mm_load1_ps(&f);
			t.m128_f32[0] = 0.0f;
			_v = _mm_add_ps(_v, t);
			return *this;
		}
		__inline CVector3 operator + (const CVector3& v) const {return (CVector3(_mm_add_ps(_v, v._v)));}
		__inline CVector3 operator + (const float f) const {return (CVector3(_mm_add_ps(_v, _mm_load_ps1(&f))));}

		/* Substraction */
		__inline CVector3& operator -= (const CVector3& v) {_v = _mm_sub_ps(_v, v._v); return *this;}
		__inline CVector3& operator -= (const float f) {_v = _mm_sub_ps(_v, _mm_load_ps1(&f)); return *this;}
		__inline CVector3 operator - (const CVector3& v) const {return (CVector3(_mm_sub_ps(_v, v._v)));}
		__inline CVector3 operator - (const float f) const {return (CVector3(_mm_sub_ps(_v, _mm_load_ps1(&f))));}

		/* Multiplication */
		__inline CVector3& operator *= (const CVector3& v) {_v = _mm_mul_ps(_v, v._v); return *this;}
		__inline CVector3& operator *= (const float f) {_v = _mm_mul_ps(_v, _mm_load_ps1(&f)); return *this;}
		__inline CVector3 operator * (const CVector3& v) const {return (CVector3(_mm_mul_ps(_v, v._v)));}
		__inline CVector3 operator * (const float f) const {return (CVector3(_mm_mul_ps(_v, _mm_load_ps1(&f))));}

		/* Division */
		__inline CVector3& operator /= (const CVector3& v) {_v = _mm_div_ps(_v, v._v); return *this;}
		__inline CVector3& operator /= (const float f) {_v = _mm_div_ps(_v, _mm_load_ps1(&f)); return *this;}
		__inline CVector3 operator / (const CVector3& v) const {return (CVector3(_mm_div_ps(_v, v._v)));}
		__inline CVector3 operator / (const float f) const {return (CVector3(_mm_div_ps(_v, _mm_load_ps1(&f))));}

		/* Dot product */
		__inline float dot(const CVector3& v) const
		{
			__m128 d;
			d = _mm_mul_ps(_v, v._v);
			d = _mm_hadd_ps(d, d);
			d = _mm_hadd_ps(d, d);

			return d.m128_f32[0];
		}

		__inline CVector3& normalize()
		{
			__m128 d;
			d = _mm_mul_ps(_v, _v);
			d = _mm_hadd_ps(d, d);
			d = _mm_hadd_ps(d, d);
//			_mm_rsqrt_ss(d)
//			d = _mm_mul_ss(_mm_rsqrt_ss(d));

			float mag2 = d.m128_f32[0];
			if (fabs(mag2) > EPSILON)
				*this /= sqrt(mag2);

			return *this;
		}

		/* Cross product */
		__inline CVector3 cross(const CVector3& v) const
		{
			__m128 tmp = _mm_sub_ps(_mm_mul_ps(_v, _mm_shuffle_ps(v._v, v._v, _MM_SHUFFLE(3, 0, 2, 1))), _mm_mul_ps(v._v, _mm_shuffle_ps(_v, _v, _MM_SHUFFLE(3, 0, 2, 1))));
			return CVector3(_mm_shuffle_ps(tmp, tmp, _MM_SHUFFLE(3, 0, 2, 1)));
		}
		//	__inline const float& x() const {return _v.m128_f32[0];}
		__inline const float x() const {return _v.m128_f32[0];}
		__inline const float y() const {return _v.m128_f32[1];}
		__inline const float z() const {return _v.m128_f32[2];}
		__inline const __m128 & getXMM() const {return _v;}
		__inline __m128 & getXMM() {return _v;}
	};
}